/*
  "bluetooth.c" from Sparkfun "i-Racer" (http://www.sparkfun.com/products/11162)
   translated to English 5/3/12 by rsp
   also updated so that it compiles using the current ImageCraft 8.05 C compiler
*/


// imagecraft libraries
#include "iom168v.h"
#include "AVRdef.h"

// application libraries
#include "mytype.h"
#include "168_PWM.h"

void delay(uchar time)  // X msec delay (X = 1 to 255 msecs)
{
    uchar i,k,j;
    for(k=0;k<time;k++)
    {
        for(j=0;j<10;j++)
        {
            for(i=0;i<248;i++);
        }
    }
}


// bluetooth signal indication is on PB0
#define blueSig_in    (DDRB &= ~(1<<BIT0))
#define blueSig_indat (PINB &   (1<<BIT0))

// LED enable is on port PC2
//  (the LEDs are multiplexed with the nRF radio control signals,
//   but in the Sparkfun i-Racer there's bluetooth instead of nRF radio)
#define LedEn_setb   PORTC |=  (1<<BIT1)
#define LedEN_clr    PORTC &= ~(1<<BIT1)
#define LedEN_out    DDRC  |=  (1<<BIT1)

// individual LED ports
#define LED1_setb   PORTC |=  (1<<BIT5)
#define LED1_clr    PORTC &= ~(1<<BIT5)
#define LED1_out    DDRC  |=  (1<<BIT5)

#define LED2_setb   PORTC |=  (1<<BIT4)
#define LED2_clr    PORTC &= ~(1<<BIT4)
#define LED2_out    DDRC  |=  (1<<BIT4)

#define LED3_setb   PORTC |=  (1<<BIT3)
#define LED3_clr    PORTC &= ~(1<<BIT3)
#define LED3_out    DDRC  |=  (1<<BIT3)

#define LED4_setb   PORTC |=  (1<<BIT2)
#define LED4_clr    PORTC &= ~(1<<BIT2)
#define LED4_out    DDRC  |=  (1<<BIT2)

#define LED5_setb   PORTD |=  (1<<BIT3)
#define LED5_clr    PORTD &= ~(1<<BIT3)
#define LED5_out    DDRD  |=  (1<<BIT3)

//----------------------------------------------PWM1---OC1
#define PWM1A_setb   PORTB |=  (1<<BIT1)
#define PWM1A_clr    PORTB &= ~(1<<BIT1)
#define PWM1A_out    DDRB  |=  (1<<BIT1)

#define PWM1B_setb   PORTB |=  (1<<BIT2)
#define PWM1B_clr    PORTB &= ~(1<<BIT2)
#define PWM1B_out    DDRB  |=  (1<<BIT2)
//---------------------------------------------PWM2----OC0
#define PWM2A_setb   PORTD |=  (1<<BIT6)
#define PWM2A_clr    PORTD &= ~(1<<BIT6)
#define PWM2A_out    DDRD  |=  (1<<BIT6)

#define PWM2B_setb   PORTD |=  (1<<BIT5)
#define PWM2B_clr    PORTD &= ~(1<<BIT5)
#define PWM2B_out    DDRD  |=  (1<<BIT5)


//-----------------------------------------------------
// Bluetooth AT commands
//
// Apparently timing is important here;
// commands must be sent out without inter-char delays,
// then must wait for bluetooth to detect EOM & respond.

// bluetooth device name (max 20 chars, no LF/CR)
uchar Name_dat[]= // "AT+NAMEDaguCar@1208"
{
0x41,0x54,0x2b,0x4e,0x41,0x4d,0x45,0x44,0x61,0x67,
0x75,0x43,0X61,0X72,0X40,0X31,0X32,0X30,0X38
};

// expected bluetooth response to AT+NAME command
uchar Name_RX[]= // "OKsetname"
{
0x4f,0x4b,0x73,0x65,0x74,0x6e,0x61,0x6d,0x65
};

// Another helpful command might be "AT+PINxxxx"
// where xxxx is the pincode; the expected response
// for this command is "OKsetpin".
//
// Otherwise, the default PIN is "1234".


//------------------------------------------------------
// subroutine to write a byte to the EEPROM
// (the ATmega48 has 256 bytes of EEPROM)

void eeprom_W(uchar w_dat,uint add)
{
    EEARH = 0;      // set address
    EEARL = add;
    EEDR  = w_dat;  // set data

    // --- begin required sequence
    EECR =  (1<<EEMPE);  // set EE write enable
    EECR |= EEPE;        // start EE write
    // --- end required sequence

    while (EECR & EEPE); // wait for write to complete
}

//------------------------------------------------------
// subroutine to read a byte from the EEPROM

uchar eeprom_R (uint add)
{
// this is necessary only if EE writes are left in progress...
//    while (EECR&0x02); // wait for previous write to complete

    EEARH = 0;         // set address
    EEARL = add;

    // read EEPROM byte
    EECR = (1<<EERE);  // start EEPROM read
    return (EEDR);    // the EEPROM result is available immediately
}


//------------------------------------------------------
// Main program start
//------------------------------------------------------

void main(void)
{
    uchar i=0,j=0,k=0,mark=0,led_mark=0;
    uint tally=0,CHA_mark=1000;

    MCUSR = 0x00;   // clear watchdog, brownout, ext reset, and POR status flags

    // initially make all pins inputs
    DDRB = 0;
    DDRC = 0;
    DDRD = 0;

    // make the default output pin value '1'
    PORTB = 0xff;
    PORTC = 0xff;
    PORTD = 0xff;

    // make the motor control pins outputs
//    PWM1A_out; //covered by PWM_RST
//    PWM1B_out; //covered by PWM_RST
//    PWM2A_out; //covered by PWM_RST
//    PWM2B_out; //covered by PWM_RST

    // initialize all motor control PWM to off
//    PWM1A_clr; //I'm not even sure what this will do, after we've set it as an output pin.
//    PWM1A_clr; //I'm not even sure what this will do, after we've set it as an output pin.
//    PWM2B_clr; //I'm not even sure what this will do, after we've set it as an output pin.
//    PWM2B_clr; //I'm not even sure what this will do, after we've set it as an output pin.

    // make bluetooth signal an input
    blueSig_in; // redundant (it's already an input)

    // make LED pins outputs
    LED1_out;
    LED2_out;
    LED3_out;
    LED4_out;
    LED5_out;
    LedEN_out;

    // enable LEDs and turn them all off
    LedEn_setb;
    LED1_clr;
    LED2_clr;
    LED3_clr;
    LED4_clr;
    // (LED5 is controlled by PWM)

    // set up USART
    UBRR0L = 51;    // set baud rate = 9600
    UCSR0B = 0x18;  // enable receive & transmit

//------------------------------------------------------------------------------------
// New product initialization & test;
// these steps are done only once when the device is first powered up.

// initialize bluetooth module name [the bluetooth module will store this in it's EEPROM]

if ((eeprom_R(1) != 0x55) || (eeprom_R(2) != 0xaa)) // EEPROM is not initialized ?
{
    // only do this once!
    eeprom_W(0x55,1);
    eeprom_W(0xaa,2);

    // turn off PWM (make motor control pins digital outputs)
    PWM_RST();
    PWM_EN(0);

    // send "AT+NAMEDaguCar@1208" to bluetooth module
    for(i=0; i<19; i++)
    {
        UDR0 = Name_dat[i];               // send char
        while (!(UCSR0A & (1<<UDRE0)));  // wait for transmit buffer to clear
    }

    // get response from bluetooth module, which should be "OKsetname"
    k=0;
    for(i=0; i<9; i++)
    {
        // if bluetooth doesn't respond,
        //   then it'll hang here forever...
        while (!(UCSR0A & (1<<RXC0)));  // wait for data ready
        j = UDR0;                       // read data char
        if (j == Name_RX[k]) k++;       // match data char to expected string
    }

    // blink LED1 8 times if the bluetooth responded as expected
    if(k > 8)
    {
        for(i=0; i < 8; i++)
        {
            LED1_setb;
            delay(100);
            LED1_clr;
            delay(100);
        }
    }

    // So at this point if the bluetooth initialization failed
    // then the device name will be whatever the factory default was.
    //
    // And there's no way to redo it without erasing the EEPROM.
}

// perform an initial electromechanical self test

if ((eeprom_R(3) != 0x89) || (eeprom_R(4) != 0x02))
{
    // only do this once!
    eeprom_W(0x89,3);
    eeprom_W(0x02,4);

    // blink LED2 20 times [ Get Ready ! ]
    for(i=0; i < 20; i++)
    {
        LED2_setb;
        delay(50);
        LED2_clr;
        delay(50);
    }

    // enable motor PWM
    PWM_RST();
    PWM_EN(1);

    // start self test sequence

    i = 0x0f; // motor speed (15 = full speed)

    motorL_reg(0,150-((i&0x0f)*10));  // go forward
    motorR_reg(0,0);  // steering full hard over in one direction
    delay(250);
    delay(250);
    delay(50);

    motorL_reg(0,150-((i&0x0f)*10));  // still going forward
    motorR_reg(1,0);  // steering full hard over in the other direction
    delay(250);
    delay(250);
    delay(50);

    motorL_reg(1,150-((i&0x0f)*10));  // reverse
    motorR_reg(0,0);  // steering back the other way again
    delay(250);
    delay(250);
    delay(50);

    motorL_reg(1,150-((i&0x0f)*10));  // more reverse
    motorR_reg(1,0);  // and now we're being pulled over for weaving
    delay(250);
    delay(250);
    delay(50);

    // turn both motors off
    motorL_reg(0,0xff);
    motorR_reg(0,0xff);
}

//------------------------------------------------------------------------------------
// Start of normal operating mode

    // set up ADC to continuously monitor the battery voltage
    ADMUX  = 0x60;  // left justified, select ADC channel 0 (PC0)
    ADCSRA = 0;
    ADCSRA |= 0x03; // prescale = 1:8
    ADCSRA |= (1 << ADATE);  // set auto trigger enable, free running mode
    ADCSRA |= (1 << ADEN);   // enable ADC module
    ADCSRA |= (1 << ADSC);   // start 1'st conversion

    // enable motor PWM mode
    PWM_RST();
    PWM_EN(1);

    // set both motors at full off
    motorL_reg(0,0xff);
    motorR_reg(0,0xff);

//--------------------------------

    // set up "breathing" LED on OC2B (it slowly fades on/off)

    TCNT2 = 0X0000;   // clear timer 2 counter
    OCR2A = 0X00FF;   // OC2A is not used (it's a digital I/O)
    OCR2B = 0X00FF;   // initial PWM value for LED

    TCCR2A = 0x31;    // OC2A normal I/O, OC2B is set on compare match, mode 1

    TIFR2  = 0xff;    // clear interrupt flags
    TIMSK2 = 0X00;    // disable OC2 interrupts

    TCCR2B = 0x01;    // select no clock prescaling

//--------------------------------

    k = 0; // last character received (unknown)

    while (1) // BIGLOOP
    {
        delay(5);  // 5 msecs
loop:
        //------------ turn off WDT --------------
        WDTCR=0X1f;
        WDTCR=0X00;
        //----------------------------------------

        while(1) // SON OF BIGLOOP
        {
            WDTCR = 0x0f;   // not quite sure what this is doing to the WDT

            // poll for receive data from bluetooth module
            if (UCSR0A & (1<<RXC0))
            {
                i = UDR0;    // get the character
                goto loop1;  // handle the character & return to "loop"
            }

            // periodically poll status and update LEDs
            tally++;
            if(tally > 1200)
            {
                //--- watch for bluetooth LOS

                i = blueSig_indat; // bluetooth signal present ?
                if(i == 0)
                {
                    motorL_reg(0,0xff);  // shut off both motors
                    motorR_reg(0,0xff);
                }

                //--- indicate the battery voltage level on the LEDs

                CHA_mark++;  // only update periodically
                if(CHA_mark > 1500)  // time to update ?
                {
                    CHA_mark = 0;

                    i = ADCH;
                    if(i < 150)
                    {   // 1/4 power, 1 LED on
                        LED1_setb;
                        LED2_clr;
                        LED3_clr;
                        LED4_clr;
                    }
                    i = ADCH;
                    if((149 < i) && (i < 158))
                    {   // 1/2 power
                        LED1_setb;
                        LED2_setb;
                        LED3_clr;
                        LED4_clr;
                    }
                    i = ADCH;
                    if((157 < i) && (i < 168))
                    {   // 3/4 power
                        LED1_setb;
                        LED2_setb;
                        LED3_setb;
                        LED4_clr;
                    }
                    i = ADCH;
                    if(i > 167)
                    {   // full power, all LEDs on
                        LED1_setb;
                        LED2_setb;
                        LED3_setb;
                        LED4_setb;
                    }
                }

                //--- watch for low battery condition

                i = ADCH;  // read battery voltage

                if(i < 142)  // test for battery low
                {
                    //------------ turn off WDT --------------
                    WDTCR=0X1f;
                    WDTCR=0X00;
                    //----------------------------------------

                    // take 20 more battery voltage samples
                    led_mark = 0;
                    for(k=0; k < 20; k++)
                    {
                        delay(1); // allow enough time for ADC to update
                        i = ADCH;
                        if(i < 142) led_mark++;
                    }
                    // if any of the samples show Ok battery voltage, then resume normal operation
                    if(led_mark < 17)goto loop;

                    //--- low battery condition has been detected

                    motorL_reg(0,0xff); // shut off both motors
                    motorR_reg(0,0xff);

                    led_mark = 1;

                    while (1) // blink all LEDs to indicate low battery
                    {
                        if (led_mark)
                        {
                            LED1_clr;
                            LED2_clr;
                            LED3_clr;
                            LED4_clr;
                            led_mark = 0;
                            delay(100);
                        }
                        else
                        {
                            LED1_setb;
                            LED2_setb;
                            LED3_setb;
                            LED4_setb;
                            led_mark = 1;
                            delay(100);

                            // watch for battery condition improving
                            //  (like maybe if it's being charged)

                            i = ADCH;
                            if(i > 160)  // got good battery voltage ?
                            {
                                // take 20 more samples to be sure
                                led_mark = 0;
                                for(k=0; k < 20; k++)
                                {
                                    delay(1);
                                    i = ADCH;
                                    if(i > 160) led_mark++;
                                }
                                if(led_mark > 17)  // 17 out of 20 is good enough
                                {
                                    // to restore normal operation,
                                    // make the WDT reset the system now
                                    WDTCR = 0x08;  // enable WDT with ~ 16 msec timeout
                                    while (1);     // HALT (until WDT fires => RESET)
                                }
                            }
                            led_mark=1;
                        }
                    }
                }

                //------------ turn off WDT --------------
                WDTCR=0X1f;
                WDTCR=0X00;
                //----------------------------------------

                tally = 0;

                //--- fade the "breathing" LED on/off alternately

                if(mark == 0)
                {
                    OCR2B++;
                    if(OCR2B > 254)
                        mark = 1;
                    else
                        mark = 0;
                }
                else
                {
                    OCR2B--;
                    if(OCR2B < 10)
                        mark = 0;
                    else
                        mark = 1;
                }
            }

        } // SON OF BIGLOOP (this loop never falls thru)


//------------------------------------------------------------------------------------
// Handle a character received by the bluetooth module
// i == reeived character

loop1:
        //------------ turn off WDT --------------
        WDTCR=0X1f;
        WDTCR=0X00;
        //----------------------------------------

        if(k == i) goto loop; // ignore duplicate characters

        k = i; // remember last char received

        WDTCR = 0x0f;

        // show speed on the LEDs (the low nybble of the command is the speed)

        if((k & 0x0f) < 2)
        {
            LED1_clr;
            LED2_clr;
            LED3_clr;
            LED4_clr;
        }
        if((1 < (k & 0x0f)) && ((k & 0x0f) < 7))
        {
            LED1_setb;
            LED2_clr;
            LED3_clr;
            LED4_clr;
        }
        if((6 < (k & 0x0f)) && ((k & 0x0f) < 11))
        {
            LED1_setb;
            LED2_setb;
            LED3_clr;
            LED4_clr;
        }
        if((10 < (k & 0x0f)) && ((k & 0x0f) < 14))
        {
            LED1_setb;
            LED2_setb;
            LED3_setb;
            LED4_clr;
        }
        if(13 < (k & 0x0f))
        {
            LED1_setb;
            LED2_setb;
            LED3_setb;
            LED4_setb;
        }

        // the high nybble of the command is the driving mode

        switch (i >> 4)
        {
            case 0:    // stop
                CHA_mark=0;
                motorL_reg(0,0xff);
                motorR_reg(0,0xff);
                break;

            case 1:    // straingt & forward
                motorL_reg(0,150-((i&0x0f)*10));
                motorR_reg(0,0xff);
                break;

            case 2:    // straignt & backward
                motorL_reg(1,150-((i&0x0f)*10));
                motorR_reg(0,0xff);
                break;

            case 3:    // left (no drive)
                motorL_reg(0,0xff);
                motorR_reg(0,0x00);
                break;

            case 4:    // right (no drive)
                motorL_reg(0,0xff);
                motorR_reg(1,0x00);
                break;

            case 5:    // left & forward
                motorL_reg(0,150-((i&0x0f)*10));
                motorR_reg(0,0);
                break;

            case 6:    // right & forward
                motorL_reg(0,150-((i&0x0f)*10));
                motorR_reg(1,0);
                break;

            case 7:    // left & backward
                motorL_reg(1,150-((i&0x0f)*10));
                motorR_reg(0,0);
                break;

            case 8:    // right & backward
                motorL_reg(1,150-((i&0x0f)*10));
                motorR_reg(1,0);
                break;
        }

    } // BIGLOOP

} // end main
